#ifndef _MULTICAM_FUSION_H
#define _MULTICAM_FUSION_H

#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <ros/ros.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <nlink_parser/LinktrackNodeframe3.h>
#include <nlink_parser/LinktrackNodeframe2.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <mavros_msgs/Altitude.h>
#include <sensor_msgs/Imu.h>
#include <string>
#include <fstream>
#include <iostream>
#include <std_msgs/Float32MultiArray.h>

// #include <tf/transform_broadcaster.h>
// #include <tf/transform_listener.h>
// #include <tf_conversions/tf_eigen.h>
// #include <tf/transform_datatypes.h> 
#include "math_tools.h"

using namespace Eigen;
using namespace std;

class dwe{
    private:
        // A and B can be a private member 
        Eigen::Matrix<double, 6, 6> A;
        Eigen::Matrix<double, 6, 3> B;
        Eigen::Matrix<double, 6, 6> AA;

        // param for MAP sliding window
		int delta_, lopt_, order_, k_, rate_;
        int vicon_flag_, param_flag_;
        int init_num_;

        // param for dwe_param
		double paramPp_, paramPv_, paramQp_, paramQv_;
		double paramR_, paramU_, paramUz_;
        double paramC_, paramCz_;
        Eigen::Matrix<double, 1, 3> kappa_;

        // param for msg_bias
		double bias_imu_;
        Eigen::Matrix<double, 1, 3> bias_noop_;
        Eigen::Matrix<double, 1, 3> bias_pos_;
        Eigen::Matrix<double, 1, 3> bias_marker_;   
        Eigen::MatrixXd bias_d_;
        Eigen::MatrixXd bias_cam_;

        vector<int> cam_id; // 初始化为-1，表示相机失效 id

        // param for cb
        std::vector<bool> cam_data_received_;
        vector<geometry_msgs::TransformStamped> cam_cbs_;

        geometry_msgs::PoseStamped VICON_posUAV_msg;
        geometry_msgs::TwistStamped VICON_velUAV_msg;
        sensor_msgs::Imu imu_msg;

    public:
        //默认构造函数
        dwe(){}
        // 构造函数
        dwe(ros::NodeHandle& nh);
        //默认析构函数
        ~dwe();

        string dataType_;

        // param for msg_sensor
        int camnum_, usedcamnum_;
        
        // [无人机中心(IMU)] 在 [VICON坐标系] 下的位置
        Vector3d pos_uav_init_;
        // [UWB0] 在 [VICON坐标系] 下的位置
        Vector3d pos_uwb01_init_;
        // [无人机中心(IMU)] 在 [UWB0坐标系] 下的位置
        Vector3d pos_uav2uwb01_init_;
        // [UWB锚点0123] 在 [UWB0坐标系] 下的位置
        Eigen::MatrixXd pos_uwb_init_;

        // param matrix
        MatrixXd paramX0_;
        MatrixXd paramIMU_;
        MatrixXd paramCAM_;

        int count_j_;
        int LOOP_num_;
        int startNum_;
        int num_state_;

        ros::Time currentTime_;
        ros::Time previousTime_ = ros::Time(0);
        int flow_quality_;
        float dt_ = 0.0;

        MatrixXd current_cam_stamp;
        MatrixXd last_cam_stamp;
        MatrixXd last_update_time;

        //ensure data input
        bool flag_init_imu_= false; 
        bool check_for_init_= false; 
        bool get_init_imu= false; 
        bool flag_init_ok_ = false; 
        bool hasTakenOff_ = false; 
        bool hasStartEst_ = false;

        //param in estimation
        MatrixXd MHE_height_;
        MatrixXd MHE_flow_;
        MatrixXd MHE_imu_, MHE_imu_residual_;
        MatrixXd MHE_imu_att_;
        Eigen::Matrix3d rotation_matrix;

        MatrixXd MHE_cam_;
        MatrixXd MHE_cam_residual_;

        MatrixXd VICON_PosUAV_;
        MatrixXd VICON_VelUAV_;
        
        MatrixXd deltaTime_;
        MatrixXd xt_;  // recommend: stl/boost list
        MatrixXd xt_real_f_;  // recommend: stl/boost list
        MatrixXd xt_f_;  // recommend: stl/boost list
        MatrixXd xt_vel_;  // recommend: stl/boost list
        MatrixXd xt_real_;
        MatrixXd x1_;  // initialize with nonzero numbers, e.g., x=y=z=uwb.dis/sqrt(3);
        MatrixXd cov_pre_xt_new;

        MatrixXd Ex,Eu,W,t_matrix,ur,xtt;
        MatrixXd ar,xxt_new,xxe,Ex_new;

        // param
        // 结构体定义
        struct FailureFlags {
            VectorXi cam;  // 相机故障标记
            int imu = 1;   // IMU故障标记

            // 构造函数，确保 VectorXd 变量被正确初始化
            FailureFlags(int cam_size) 
                : cam(VectorXi::Zero(cam_size)) {}
        };

        // 质量参数结构体
        struct QualityParams {
            MatrixXd cam;  // 相机故障标记
            Vector3d imu;    // IMU三维质量
            Vector3d imu_p;    // IMU三维质量
            Vector3d imu_v;    // IMU三维质量
            double sum = 0.0;

        // 构造函数，确保 VectorXd 变量被正确初始化
            QualityParams(int cam_size) 
                : cam(MatrixXd::Zero(3,cam_size)), 
                imu(Vector3d::Zero()),
                imu_p(Vector3d::Zero()),
                imu_v(Vector3d::Zero()) {}
        };

        // 主参数结构体
        struct SensorParams {
            FailureFlags fail;
            QualityParams param; 
            QualityParams weight;  

            // 默认构造函数，防止编译错误
            SensorParams() : fail(0), param(0), weight(0) {}
            // 初始化构造函数
            SensorParams(int cam_size) 
                : fail(cam_size), param(cam_size),
                weight(cam_size) {}
        };
        double residual_threshold = 1e-6;
        
        SensorParams sensor;

    public:
        // declaration for functions
        // void broadcast_base_to_coopestimation();
        // void uwb3_cb(nlink_parser::LinktrackNodeframe3 msg);
        void imu_new_cb(const sensor_msgs::Imu::ConstPtr &msg);

        // define the input data form vicon
        void pos_uav_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void vel_uav_cb(const geometry_msgs::TwistStamped::ConstPtr &msg);

        void camA_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);
        void camB_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);
        void camC_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);
        void camD_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);
        void camA_a2g_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);
        void camTAG_cb(const geometry_msgs::TransformStamped::ConstPtr &msg);

        // void data_writter();

        void Initialdataxt();
        void InitialWithParam();
        void solveonce();
        void update_data();
        void initializeEstimation();
};

int data_loss_detection(const Eigen::MatrixXd& MHE_data_, double loss_epsilon);

#endif
